
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       pid.c
  * @author     baiyang
  * @date       2021-8-8
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include <stdlib.h>

#include "pid.h"
#include <common/time/gp_time.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void pid_ctrl_ctor(PID_ctrl * controler, float kp, float ki, float kd, float kff, float kimax, float filt_t_hz, float filt_e_hz, float filt_d_hz, float initial_dt)
{
    controler->_kp = kp;
    controler->_ki = ki;
    controler->_kd = kd;
    controler->_kff = kff;
    controler->_kimax = fabsf(kimax);
    controler->_filt_t_hz = fabsf(filt_t_hz);
    controler->_filt_e_hz = fabsf(filt_e_hz);
    controler->_filt_d_hz = fabsf(filt_d_hz);

    controler->dt = initial_dt;
    controler->integrator = 0;
    controler->error = 0;
    controler->derivative = 0;

    // reset input filter to first value received
    controler->_flags_reset_filter = true;
}

//  update_all - set target and measured inputs to PID controller and calculate outputs
//  target and error are filtered
//  the derivative is then calculated and filtered
//  the integral is then updated based on the setting of the limit flag
float pid_ctrl_update_all(PID_ctrl * controler, float target, float measurement, bool limit)
{
    // don't process inf or NaN
    if (!isfinite(target) || !isfinite(measurement)) {
        return 0.0f;
    }

    // reset input filter to value received
    if (controler->_flags_reset_filter) {
        controler->_flags_reset_filter = false;
        controler->target = target;
        controler->error = controler->target - measurement;
        controler->derivative = 0.0f;
    } else {
        float error_last = controler->error;
        controler->target += pid_ctrl_get_filt_t_alpha(controler) * (target - controler->target);
        controler->error += pid_ctrl_get_filt_e_alpha(controler) * ((controler->target - measurement) - controler->error);

        // calculate and filter derivative
        if (controler->dt > 0.0f) {
            float derivative = (controler->error - error_last) / controler->dt;
            controler->derivative += pid_ctrl_get_filt_d_alpha(controler) * (derivative - controler->derivative);
        }
    }

    // update I term
    pid_ctrl_update_i(controler,limit);

    float P_out = (controler->error * controler->_kp);
    float D_out = (controler->derivative * controler->_kd);

    return P_out + controler->integrator + D_out;

}

//  update_error - set error input to PID controller and calculate outputs
//  target is set to zero and error is set and filtered
//  the derivative then is calculated and filtered
//  the integral is then updated based on the setting of the limit flag
//  Target and Measured must be set manually for logging purposes.
// todo: remove function when it is no longer used.
float pid_ctrl_update_error(PID_ctrl * controler, float error, bool limit)
{
    // don't process inf or NaN
    if (!isfinite(error)) {
        return 0.0f;
    }

    controler->target = 0.0f;

    // reset input filter to value received
    if (controler->_flags_reset_filter) {
        controler->_flags_reset_filter = false;
        controler->error = error;
        controler->derivative = 0.0f;
    } else {
        float error_last = controler->error;
        controler->error += pid_ctrl_get_filt_e_alpha(controler) * (error - controler->error);

        // calculate and filter derivative
        if (controler->dt > 0.0f) {
            float derivative = (controler->error - error_last) / controler->dt;
            controler->derivative += pid_ctrl_get_filt_d_alpha(controler) * (derivative - controler->derivative);
        }
    }

    // update I term
    pid_ctrl_update_i(controler,limit);

    float P_out = (controler->error * controler->_kp);
    float D_out = (controler->derivative * controler->_kd);

    return P_out + controler->integrator + D_out;

}

//  update_i - update the integral
//  if the limit flag is set the integral is only allowed to shrink
void pid_ctrl_update_i(PID_ctrl * controler, bool limit)
{
    if (!math_flt_zero(controler->_ki) && math_flt_positive(controler->dt)) {
        // Ensure that integrator can only be reduced if the output is saturated
        if (!limit || ((math_flt_positive(controler->integrator) && math_flt_negative(controler->error)) || (math_flt_negative(controler->integrator) && math_flt_positive(controler->error)))) {
            controler->integrator += ((float)controler->error * controler->_ki) * controler->dt;
            controler->integrator = math_constrain_float(controler->integrator, -controler->_kimax, controler->_kimax);
        }
    } else {
        controler->integrator = 0.0f;
    }
}

// reset_I_smoothly - reset the integrator
void pid_ctrl_reset_i_smoothly(PID_ctrl * controler)
{
    controler->integrator = 0;
    float reset_time = PID_RESET_TC * 3.0f;
    uint64_t now = time_millis();

    if ((now - controler->reset_last_update) > 5e5 ) {
        controler->reset_counter = 0;
    }
    if ((float)controler->reset_counter < (reset_time/controler->dt)) {
        controler->integrator = controler->integrator - (controler->dt / (controler->dt + PID_RESET_TC)) * controler->integrator;
        controler->reset_counter++;
    } else {
        controler->integrator = 0;
    }
    controler->reset_last_update = now;
}

float pid_ctrl_get_filt_alpha(PID_ctrl * controler, float filt_hz)
{
    if (math_flt_zero(filt_hz)) {
        return 1.0f;
    }

    // calculate alpha
    float rc = 1 / (M_2PI * filt_hz);
    return controler->dt / (controler->dt + rc);
}

float pid_ctrl_get_filt_t_alpha(PID_ctrl * controler)
{
    return pid_ctrl_get_filt_alpha(controler,controler->_filt_t_hz);
}

float pid_ctrl_get_filt_e_alpha(PID_ctrl * controler)
{
    return pid_ctrl_get_filt_alpha(controler,controler->_filt_e_hz);
}

float pid_ctrl_get_filt_d_alpha(PID_ctrl * controler)
{
    return pid_ctrl_get_filt_alpha(controler,controler->_filt_d_hz);
}

// integrator setting functions
void pid_ctrl_set_integrator3(PID_ctrl * controler, float target, float measurement, float i)
{
    pid_ctrl_set_integrator2(controler, target - measurement, i);
}

void pid_ctrl_set_integrator2(PID_ctrl * controler, float error, float i)
{
    controler->integrator = math_constrain_float(i - error * controler->_kp, -controler->_kimax, controler->_kimax);
}

void pid_ctrl_set_integrator1(PID_ctrl * controler, float i)
{
    controler->integrator = math_constrain_float(i, -controler->_kimax, controler->_kimax);
}

/*------------------------------------test------------------------------------*/


